/*
 *  polygonizer3.h
 *  Fracture
 *
 *  Created by Jamie Portsmouth on 24/09/2011.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */


#ifndef POLYGONIZER3_H
#define POLYGONIZER3_H

#if 0

#include <list>
#include <vector>
#include <queue>
#include "Vec4.h"
#include "Matrix3.h"
#include "Aabb.h"
#include "float.h"
#include "Geometry.h"
#include <cmath>

#include "AabbTree.h"


	// FOR DEBUGGING PURPOSES
#include <OpenGL/OpenGL.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>

#include <stdio.h>


/* note #undef's at end of file */
#define IM1 2147483563
#define IM2 2147483399
#define AM (1.0/IM1)
#define IMM1 (IM1-1)
#define IA1 40014
#define IA2 40692
#define IQ1 53668
#define IQ2 52774
#define IR1 12211
#define IR2 3791
#define NTAB 32
#define NDIV (1+IMM1/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)

float ran2(long *idum)
{
	int j;
	long k;
	static long idum2=123456789;
	static long iy=0;
	static long iv[NTAB];
	float temp;
	
	if (*idum <= 0) {
		if (-(*idum) < 1) *idum=1;
		else *idum = -(*idum);
		idum2=(*idum);
		for (j=NTAB+7;j>=0;j--) {
			k=(*idum)/IQ1;
			*idum=IA1*(*idum-k*IQ1)-k*IR1;
			if (*idum < 0) *idum += IM1;
			if (j < NTAB) iv[j] = *idum;
		}
		iy=iv[0];
	}
	k=(*idum)/IQ1;
	*idum=IA1*(*idum-k*IQ1)-k*IR1;
	if (*idum < 0) *idum += IM1;
	k=idum2/IQ2;
	idum2=IA2*(idum2-k*IQ2)-k*IR2;
	if (idum2 < 0) idum2 += IM2;
	j=iy/NDIV;
	iy=iv[j]-idum2;
	iv[j] = *idum;
	if (iy < 1) iy += IMM1;
	if ((temp=AM*iy) > RNMX) return RNMX;
	else return temp;
}
#undef IM1
#undef IM2
#undef AM
#undef IMM1
#undef IA1
#undef IA2
#undef IQ1
#undef IQ2
#undef IR1
#undef IR2
#undef NTAB
#undef NDIV
#undef EPS
#undef RNMX


static inline float edgeEdgeDistanceSqr(const V3f& E0_sta, const V3f& E0_end, const V3f& E1_sta, const V3f& E1_end,
										float &s, float&t, V3f& c1, V3f& c2)
{			
		// From Ericson.
	const Imath::V3f& p1 = E0_sta;
	const Imath::V3f& q1 = E0_end;
	const Imath::V3f& p2 = E1_sta;
	const Imath::V3f& q2 = E1_end;
	V3f d1 = q1 - p1; // Direction vector of segment S1
	V3f d2 = q2 - p2; // Direction vector of segment S2
	V3f r = p1 - p2;
	float a = d1 ^ d1; // Squared length of segment S1, always nonnegative
	float e = d2 ^ d2; // Squared length of segment S2, always nonnegative
	float f = d2 ^ r;
	
	if (a<=FLT_EPSILON && e<=FLT_EPSILON)
	{	// Both segments degenerate into points
		s = t = 0.0f;
		c1 = p1;
		c2 = p2;
		return (c1-c2)^(c1-c2);
	}
	
	if (a<=FLT_EPSILON)
	{   // First segment degenerates into a point
		s = 0.0f;
		t = f/e;
		t = clamp(t, 0.0f, 1.0f);
	}
	else
	{
		float c = d1^r;
		if (e<=FLT_EPSILON)
		{   // Second segment degenerates into a point
			t = 0.0f;
			s = clamp(-c/a, 0.0f, 1.0f);
		}
		else
		{   // The general nondegenerate case starts here
			float b = d1^d2;
			float denom = a*e - b*b;
			
				// If segments non parallel, compute closest point on L1 to L2 and
				// clamp to segment S1. Else pick arbitrary s (here 0)
			if (denom != 0.0f)
				s = clamp((b*f - c*e)/denom, 0.0f, 1.0f);
			else 
				s = 0.0f;
			
				// Compute point on L2 closest to S1(s)
			t = (b*s + f)/e;
			
				// If t in [0,1] done. Else clamp t, recompute s for the new value of t
				// and clamp s to [0,1]
			if (t<0.0f)
			{
				t = 0.0f;
				s = clamp(-c/a, 0.0f, 1.0f);
			}
			else if (t>1.0f) 
			{
				t = 1.0f;
				s = clamp((b-c)/a, 0.0f, 1.0f);
			}
		}
	}
	c1 = p1 + d1*s;
	c2 = p2 + d2*t;
	return (c1-c2)^(c1-c2);
}


class Polygonizer 
{
public:
	
	// Pass the functor which defines the level set as the zero value isosurface
	template<typename Functor>
	void polygonize(const Box3f& bbox, float tolerance, float scale=0.1f);
	
	const Geometry& getGeometry()
	{
		return m_mesh;
	}
	
	
	struct ExampleLevelSetFunctor
	{
			// Level set at value=0.0
		static float value(V3f P)
		{
			float X = P[0];
			float Y = P[1];
			float Z = P[2];
			
			V3f A = P - V3f(-0.5, 0, 0);
			V3f B = P - V3f(0.5, 0, 0);
			return 1.f/A.length() + 1.f/B.length() - 1.0f;
		}
	};
	
	
	struct FatEdge
	{
		FatEdge() : m_a(-1), m_b(-1) {}
		
			// make fat edge from existing mesh endpoints
		FatEdge(int a, int b, const Geometry& mesh, const vector<float>& vertexRocs) : m_a(a), m_b(b)
		{
			m_A = mesh.m_vertices[a];
			m_B = mesh.m_vertices[b];
			m_rA = vertexRocs[a];
			m_rB = vertexRocs[b];
			makeBox();
		}
		
			// make fat edge from one existing mesh endpoint, and one new point
		FatEdge(int a, const V3f& B, float rB, const Geometry& mesh, const vector<float>& vertexRocs) : m_a(a), m_b(-1)
		{
			m_A = mesh.m_vertices[a];
			m_B = B;
			m_rA = vertexRocs[a];
			m_rB = rB;
			makeBox();
		}
		
			// make fat edge from two new points
		FatEdge(const V3f& A, const V3f& B, float rA, float rB) : m_a(-1), m_b(-1)
		{
			m_A = A;
			m_B = B;
			m_rA = rA;
			m_rB = rB;
			makeBox();
		}
		
		inline void makeBox()
		{
			V3f vA(m_rA), vB(m_rB);
			Box3f boxA, boxB;
			boxA.extendBy(m_A); boxB.extendBy(m_B);
			boxA.max += vA; boxA.min -= vA;
			boxB.max += vB; boxB.min -= vB;
			m_bbox.extendBy(boxA);
			m_bbox.extendBy(boxB);
		}
		
		int m_a, m_b;
		V3f m_A, m_B;
		float m_rA, m_rB;
		Box3f m_bbox;
	};
	
	
	struct Front
	{
		struct Vertex
		{
			int m_index;
			bool m_active;
			float m_openingAngle;
			
			bool operator<(const Vertex& other) const
			{
				return m_openingAngle < other.m_openingAngle;
			}
			
			Vertex(int index) : m_index(index), m_active(true) { }
		};
		
			// Circular linked list of vertices in the growing front
		list<Vertex> m_vertices;
		
		map<int, list<Vertex>::iterator> m_vertexIndexToFrontIterator;
		
		AabbTree<FatEdge> m_fatEdgeBVH;
		
		
		bool candidateEdgeFits(const FatEdge& candidate, FatEdge& hitEdge, const Geometry& mesh, float& minEEdist, float tol=1.0f) const
		{
			vector<int> leafIndices;
			m_fatEdgeBVH.query(candidate.m_bbox, leafIndices);
			
			bool edgeFits = true;
			minEEdist = FLT_MAX;
			
			for (int l=0; l<leafIndices.size(); ++l)
			{
				const FatEdge& edge = m_fatEdgeBVH.getLeafData(leafIndices[l]);
				
				// We ignore this edge if it shares a vertex with the candidate (such a 'hit' does not count as a collision obviously)
				if ( edge.m_a==candidate.m_a || edge.m_a==candidate.m_b ||
					 edge.m_b==candidate.m_a || edge.m_b==candidate.m_b )
				{
					continue;
				}
				
				// Narrowphase - does edge 'overlap' candidate?
				// We do the narrowphase by getting the closest points on the two unexpanded edges, then 
				// we consider it a hit if the distance between the points is less than the sum of the r.o.c at the points
				// (where the r.o.c. at the points is just obtained by linear interpolation of the r.o.c at the endpoint vertices).
				const V3f& EA = edge.m_A;
				const V3f& EB = edge.m_B;
				const V3f& CA = candidate.m_A;
				const V3f& CB = candidate.m_B;
				
				float s, t;
				V3f c1, c2;
				float eeDistSqr = edgeEdgeDistanceSqr(EA, EB, CA, CB, s, t, c1, c2);
				float eeDist = sqrt(eeDistSqr);
				
				if (eeDist < minEEdist)
				{
					minEEdist = eeDist;
					hitEdge = edge;
				}
				
				float EA_interp_r = s*edge.m_rA + (1.f-s)*edge.m_rB;
				float EB_interp_r = t*candidate.m_rA + (1.f-t)*candidate.m_rB;
				float minDist = sqrt(EA_interp_r*EA_interp_r + EB_interp_r*EB_interp_r + 2.f*EA_interp_r*EB_interp_r);
				const float proxim = 0.5f;
				
				if ( eeDist < tol * proxim * minDist ) 
				{
					edgeFits = false;
				}
			}
			
			return edgeFits;
		}
		
		
		void rebuildFatEdgeBVH(const Geometry& mesh, const vector<float>& vertexRocs)
		{
			list<Vertex>::iterator vertex = m_vertices.begin();
			
			vector<Box3f> leafAabbs;
			vector<FatEdge> leafDatas;
			
			while (vertex != m_vertices.end())
			{
				list<Vertex>::iterator nextVertex = vertex;
				nextVertex++;
				if (nextVertex == m_vertices.end()) nextVertex++;
				
				int vertexA = vertex->m_index;
				int vertexB = nextVertex->m_index;
				FatEdge edge(vertexA, vertexB, mesh, vertexRocs);
				
				const V3f& vA = mesh.m_vertices[vertexA];
				const V3f& vB = mesh.m_vertices[vertexB];
				
				Box3f box;
				box.extendBy(vA);
				box.extendBy(vB);
				leafAabbs.push_back(box);
				leafDatas.push_back(edge);
				
				vertex++;
			}
			
			m_fatEdgeBVH.clear();
			m_fatEdgeBVH.buildTreeTopDown(leafAabbs, leafDatas);
		}
		
	};
	
	
	/**************************************  data members  **********************************************************/
	
	vector<Front*> m_fronts;
	vector<float> m_vertexRocs;
	Geometry m_mesh;
	
	float m_tol;
	float m_scale;
	long m_seed;
	
	// DEBUG LOGIC FOR TESTS
	bool m_switchFront;
	int m_currentFront;
	list<Front::Vertex>::iterator m_currentFrontVertex;
	
	/************************************************************************************************/
	
	// Global edge collision check, against all fronts
	bool candidateEdgeFits(const FatEdge& candidate, int& hitFront, FatEdge& hitEdge, float tol=1.0f) const
	{
		bool edgeFits = true;
		float minEEdist = FLT_MAX;
		
		for (int f=0; f<m_fronts.size(); ++f)
		{
			const Front* front = m_fronts[f];
			
			float front_minEEdist;
			if (!front->candidateEdgeFits(candidate, hitEdge, m_mesh, front_minEEdist, tol))
			{
				edgeFits = false;
			}
			
			if (front_minEEdist < minEEdist)
			{
				minEEdist = front_minEEdist;
				hitFront = f;
			}
			
		}
		return edgeFits;
	}
	
	static inline void computeTangentSpace(const V3f& N, V3f& T, V3f& B)
	{
		// (N assumed normalized)
		float nx = N[0];
		float ny = N[1];
		float nz = N[2];
		if (nx>0.5f || ny>0.5f) 
			T = V3f(ny, -nx, 0.f);
		else
			T = V3f(-nz, 0.f, nx);
		B = N % T;
		T.normalize();
		B.normalize();
	}
	
	static inline void getDeltas(double& hX, double& hY, double& hZ, const Imath::V3d& wsP, double eps)
    {
		// Numerical recipe to ensure that hX, hY, hZ are 'good' values that minimize round-off error
		// (see http://en.wikipedia.org/wiki/Numerical_differentiation)
        double X = abs(wsP[0]);
        double Y = abs(wsP[1]);
        double Z = abs(wsP[2]);
        hX = eps * std::max(X, 1.0);
        hY = eps * std::max(Y, 1.0);
        hZ = eps * std::max(Z, 1.0);
        volatile double tempX = X + hX;
        volatile double tempY = Y + hY;
        volatile double tempZ = Z + hZ;
        hX = tempX - X;
        hY = tempY - Y;
        hZ = tempZ - Z;
    }
	
	template<typename Functor>
	inline V3d gradient(const V3d& P)
	{
		static const double eps = static_cast<double>(pow(static_cast<double>(DBL_EPSILON), 1.0/3.0));
		double hX, hY, hZ;
		getDeltas(hX, hY, hZ, P, eps);
		
		V3d dX(hX, 0.0, 0.0);
		V3d dY(0.0, hY, 0.0);
		V3d dZ(0.0, 0.0, hZ);
		double inv2hX = 0.5/hX;
		double inv2hY = 0.5/hY;
		double inv2hZ = 0.5/hZ;
		
		double Vxp = Functor::value(P + dX);
		double Vxm = Functor::value(P - dX);
		double Vyp = Functor::value(P + dY);
		double Vym = Functor::value(P - dY);
		double Vzp = Functor::value(P + dZ);
		double Vzm = Functor::value(P - dZ);
		double gradX = inv2hX * (Vxp - Vxm);
		double gradY = inv2hY * (Vyp - Vym);
		double gradZ = inv2hZ * (Vzp - Vzm);
		return V3d(gradX, gradY, gradZ);
	}
	
	template<typename Functor>
	inline M33d hessian(const V3d& P)
	{
		static const double eps = static_cast<double>(pow(static_cast<double>(DBL_EPSILON), 1.0/3.0));
		double hX, hY, hZ;
		getDeltas(hX, hY, hZ, P, eps);
		
		V3d dX(hX, 0.0, 0.0);
		V3d dY(0.0, hY, 0.0);
		V3d dZ(0.0, 0.0, hZ);
		
		V3d Gxp = gradient<Functor>(P + dX);
		V3d Gxm = gradient<Functor>(P - dX);
		V3d Gyp = gradient<Functor>(P + dY);
		V3d Gym = gradient<Functor>(P - dY);
		V3d Gzp = gradient<Functor>(P + dZ);
		V3d Gzm = gradient<Functor>(P - dZ);
		
		double inv2hX = 0.5/hX;
		double inv2hY = 0.5/hY;
		double inv2hZ = 0.5/hZ;
		double hXX = inv2hX*(Gxp[0] - Gxm[0]);
		double hYY = inv2hY*(Gyp[1] - Gym[1]); 
		double hZZ = inv2hZ*(Gzp[2] - Gzm[2]);
		double hXY = 0.5*(inv2hX*(Gxp[1] - Gxm[1]) + inv2hY*(Gyp[0] - Gym[0]));
		double hYZ = 0.5*(inv2hY*(Gyp[2] - Gym[2]) + inv2hZ*(Gzp[1] - Gzm[1]));
		double hXZ = 0.5*(inv2hZ*(Gzp[0] - Gzm[0]) + inv2hX*(Gxp[2] - Gxm[2]));
		
		V3d col0(hXX, hXY, hXZ);
		V3d col1(hXY, hYY, hXY);
		V3d col2(hXZ, hYZ, hZZ);
		M33d H;
		
		H[0][0] = hXX;
		H[1][0] = hXY;
		H[2][0] = hXZ;
		H[0][1] = hXY;
		H[1][1] = hYY;
		H[2][1] = hYZ;
		H[0][2] = hXZ;
		H[1][2] = hYZ;
		H[2][2] = hZZ;
		return H;
	}
	
	inline double trace(const M33d& F)
	{
		return F[0][0] + F[1][1] + F[2][2];
	}
	
	inline M33d adjoint(const M33d& F)
	{
		double Fxx = F[0][0];
		double Fyy = F[1][1];
		double Fzz = F[2][2];
		double Fxy = F[0][1];
		double Fxz = F[0][2];
		double Fyz = F[1][2];
		M33d A;
		A[0][0] = Fyy*Fzz - Fyz*Fyz;
		A[1][1] = Fxx*Fzz - Fxz*Fxz;
		A[2][2] = Fxx*Fyy - Fxy*Fxy;
		A[1][0] = Fyz*Fxz - Fxy*Fzz;
		A[2][0] = Fxy*Fyz - Fyy*Fxz;
		A[2][1] = Fxy*Fxz - Fxx*Fyz;
		A[0][1] = F[1][0];
		A[0][2] = F[2][0];
		A[1][2] = F[2][1];
		return A;
	}
	
	
	template<typename Functor>
	void calcCurvature(const V3d& P, double& k1, double& k2)
	{
		// Following "Curvature formulas for implicit curves and surfaces" by Ron Goldman
		V3d gradF = gradient<Functor>(P);
		M33d H = hessian<Functor>(P);
		M33d Ha = adjoint(H);
		double gradFSqr = gradF ^ gradF;
		double gaussian = (gradF^(gradF*Ha)) / (gradFSqr*gradFSqr + DBL_EPSILON);
		double mean = ((gradF^(gradF*H)) - gradFSqr*trace(H)) / (2.0*gradFSqr*gradF.length() + DBL_EPSILON);
		double det = sqrt(abs(mean*mean - gaussian));
		k1 = mean + det;
		k2 = mean - det;
	}
	
	
	template<typename Functor>
	float rocAtPoint(const V3d& P)
	{
		// Following "Curvature formulas for implicit curves and surfaces" by Ron Goldman
		double k1, k2; 
		calcCurvature<Functor>(P, k1, k2);
		
		double roc = m_scale / (max(abs(k1), abs(k2)) + FLT_EPSILON);
		double maxRoc = 0.03f * min(min(m_extents[0], m_extents[1]), m_extents[2]);
		return min(roc, maxRoc);
	}
	
	
	Polygonizer(long seed=-1) : m_seed(seed)
	{
		ran2(&m_seed);	
	}
	
	
	template<typename Functor>
	V3f project(const V3f& start, float tol, int maxIterations=32)
	{
		float tolSqr = tol*tol;
		
		V3f x = start;
		float f = Functor::value(x);
		float fSqr = f*f;
		int nIter = 0;
		
		// Take Newton steps until the function magnitude falls below tolerance
		while (fSqr>tolSqr && nIter<maxIterations)
		{
			// (for the moment, ignore the issue of what to do if the gradient is close to zero).
			V3f grad = gradient<Functor>(x);
			//if ( (grad^grad) < tol ) { printf("project() failed, gradient magnitude too small"); exit(1); }
			
			// Take a Newton step 
			V3f step = f*grad/(grad^grad);
			
			// Reduce step size until the function magnitude falls, if needed 
			float fp, fpSqr;
			V3f xp;
			do 
			{
				nIter++;
				xp = x - step;
				fp = Functor::value(xp);
				fpSqr = fp*fp;
				step *= 0.5f;
			}
			while (fpSqr>=fSqr && nIter<maxIterations);
			
			x = xp;
			f = fp;
			fSqr = fpSqr;
		}
		
		if (nIter>=maxIterations) 
		{ 
			printf("warning, too many iterations in project"); 
				//exit(1); 
		}
		
		return x;
	}
	
	
	void computeOpeningAngle(list<Front::Vertex>::iterator vertex, Front* front)
	{
		list<Front::Vertex>::const_iterator prevVertex = vertex; 
		prevVertex--;
		if (prevVertex == front->m_vertices.end()) prevVertex--;
		
		list<Front::Vertex>::const_iterator nextVertex = vertex; 
		nextVertex++;
		if (nextVertex == front->m_vertices.end()) nextVertex++;
		
		const V3f& V = m_mesh.m_vertices[vertex->m_index];
		const V3f& N = m_mesh.m_vertexNormals[vertex->m_index];
		
		const V3f& Vprev = m_mesh.m_vertices[prevVertex->m_index];
		const V3f& Vnext = m_mesh.m_vertices[nextVertex->m_index];
		
		V3f X, Y;
		computeTangentSpace(N, X, Y);
		
		V3f Vprev_project(0);
		Vprev_project.x = (V - Vprev) ^ X;
		Vprev_project.y = (V - Vprev) ^ Y;
		Vprev_project.normalize();
		
		V3f Vnext_project(0);
		Vnext_project.x = (Vnext - V) ^ X;
		Vnext_project.y = (Vnext - V) ^ Y;
		Vnext_project.normalize();
		
		float dot = clamp(Vprev_project ^ Vnext_project, -1.f+FLT_EPSILON, 1.f-FLT_EPSILON);
		
		float openingAngle;
		if ((Vprev_project % Vnext_project).z >= 0.0f)
		{
				// reflex angle
			openingAngle = M_PI + acos(dot);
		}
		else
		{
				// acute angle
			openingAngle = M_PI - acos(dot); 
		}
		
		vertex->m_openingAngle = openingAngle;
	}
	
	
	

	static inline void drawArrow(const V3f& posA, const V3f& posB, const V3f& color, float scale)
	{
		V3f vec = posB - posA;
		V3f t0, t1, t2;
		computeTangentSpace(vec, t1, t2);
		
		t0 = posB - posA;
		t0.normalize();
		
		V3f lobe1  = posB - t0*scale + t1*scale;
		V3f lobe2  = posB - t0*scale - t1*scale;
		V3f lobe3  = posB - t0*scale + t2*scale;
		V3f lobe4  = posB - t0*scale - t2*scale;
		
		V3f v3ArrowShape[] = {
			V3f(posA), V3f(posB),
			V3f(posB), V3f(lobe1),
			V3f(posB), V3f(lobe2),
			V3f(posB), V3f(lobe3),
			V3f(posB), V3f(lobe4),
		};
		
		glDisable(GL_LIGHTING);
		glLineWidth(1.5f);
		glColor4f(color.x,color.y,color.z,1.0f);
		glEnableClientState(GL_VERTEX_ARRAY);
		glVertexPointer(3, GL_FLOAT, sizeof(V3f), &v3ArrowShape[0].x);
		glDrawArrays(GL_LINES, 0, sizeof(v3ArrowShape)/sizeof(V3f));
		glDisableClientState(GL_VERTEX_ARRAY);
		glColor4f(1.0f,1.0f,1.0f,1.0f);
	}
	
	
	void debug_printFronts()
	{
		printf("\n\n################## fronts ####################\n");
		for (int f=0; f<m_fronts.size(); ++f)
		{
			printf("\n------- front %d --------\n", f);
			Front* front = m_fronts[f];
			list<Front::Vertex>::iterator iter = front->m_vertices.begin();
			while (iter != front->m_vertices.end())
			{
				printf("%d -> ", iter->m_index);
				iter++;
			}
			printf("\n");

			printf("Check vertex->iterator map:\n");
			std::map<std::string, std::string>::iterator itermap;
			{
				std::map<int, list<Front::Vertex>::iterator>::iterator itermap;
				for (itermap  = front->m_vertexIndexToFrontIterator.begin(); 
					 itermap != front->m_vertexIndexToFrontIterator.end(); 
					 itermap++) 
				{
					int vertexIndex = itermap->first;
					list<Front::Vertex>::iterator vertex = itermap->second;
					printf("key, val->m_index: %d %d\n", vertexIndex, vertex->m_index);
				}
			}
		}
		
		printf("\n\n");
		for(int vi=0; vi<m_vertexRocs.size(); ++vi)
		{
			printf("Vertex %d roc = %f\n", vi, m_vertexRocs[vi]);
		}
		printf("\n\n");
	}
	
	
	void drawFronts() const
	{
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		
		glDisable(GL_TEXTURE_2D);
		glEnable(GL_DEPTH_TEST);
		glDisable(GL_CULL_FACE);
		glCullFace(GL_BACK);
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
		glDisable(GL_LIGHTING);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		
		for (int f=0; f<m_fronts.size(); ++f)
		{
			float fr = float(f)/(m_fronts.size());
			
			const Front* front = m_fronts[f];
			list<Front::Vertex>::const_iterator vertex = front->m_vertices.begin();
			while (vertex != front->m_vertices.end())
			{
				list<Front::Vertex>::const_iterator nextVertex = vertex; 
				nextVertex++;
				if (nextVertex == front->m_vertices.end()) nextVertex++;
				
				const Vec4& V = m_mesh.m_vertices[vertex->m_index];
				const Vec4& Vnext = m_mesh.m_vertices[nextVertex->m_index];
				float length = (V-Vnext).length3();
				
				const Vec4& N = m_mesh.m_vertexNormals[vertex->m_index];
				drawArrow(V, V+0.1f*length*N, V3f(0,0,0), length*0.01f);
				Vec4 dV = 0.1f*(Vnext-V);
				drawArrow(V+dV, Vnext-dV, V3f(1.0-fr,0,fr), length*0.1f);
				
				if (m_currentFrontVertex==vertex)
				{
					DrawUtils::drawCrosshairs(length*0.15f, V, 1.f, 1.f, 0.f, 0.6f);
				}
				if (vertex->m_active)
				{
					DrawUtils::drawCrosshairs(length*0.1f, V, 1.f, 0.f, 0.f, 0.6f);
				}
				else
				{
					DrawUtils::drawCrosshairs(length*0.1f, V, 0.f, 0.f, 1.f, 0.6f);
				}	
			
				vertex++;
			}
			glEnd();
		}
		
		glPopAttrib();
	}
	
	inline V3f rotateZ(const V3f& vIn, float angleRadians)
	{
		float s = sin(angleRadians);
		float c = cos(angleRadians);
		V3f vOut;
		vOut.x = c*vIn.x - s*vIn.y;
		vOut.y = s*vIn.x + c*vIn.y;
		vOut.z = 0.0f;
		return vOut;
	}
	
	
	bool fillHole(Front* F)
	{
		vector<int> indices;
		list<Front::Vertex>::const_iterator vertex = F->m_vertices.begin();
		while (vertex != F->m_vertices.end())
		{	
			indices.push_back(vertex->m_index);
			vertex++;
		}
		
		//assert(indices.size()>=3);
		if (indices.size()==3)
		{
			// Add new triangle to mesh
			{
				Geometry::Triangle tri;
				tri.m_vertex[0] = indices[0];
				tri.m_vertex[1] = indices[1];
				tri.m_vertex[2] = indices[2];
				m_mesh.m_triangles.push_back(tri);
			}
			// Rebuild the front BVH
			F->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);
		}
		else if (indices.size()==4)
		{
			// Add new triangles to mesh
			{
				Geometry::Triangle tri;
				tri.m_vertex[0] = indices[0];
				tri.m_vertex[1] = indices[1];
				tri.m_vertex[2] = indices[2];
				m_mesh.m_triangles.push_back(tri);
			}
			{
				Geometry::Triangle tri;
				tri.m_vertex[0] = indices[2];
				tri.m_vertex[1] = indices[3];
				tri.m_vertex[2] = indices[0];
				m_mesh.m_triangles.push_back(tri);
			}
			
			// Rebuild the front BVH
			F->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);
		}	
	}
	
	
	bool splitOrMerge(int currentFront, list<Front::Vertex>::iterator& frontVertex, int hitFront, const FatEdge& hitEdge)
	{
		Front* front = m_fronts[currentFront];
		assert(front->m_vertices.size()>4);
				
		list<Front::Vertex>::iterator prevVertex = frontVertex; prevVertex--;
		if (prevVertex == front->m_vertices.end()) prevVertex--;
		
		list<Front::Vertex>::iterator nextVertex = frontVertex; nextVertex++;
		if (nextVertex == front->m_vertices.end()) nextVertex++;
		
		// Get the vertices of the hit edge on the colliding front.
		Front* otherFront = m_fronts[hitFront];
		list<Front::Vertex>::iterator A = otherFront->m_vertexIndexToFrontIterator[hitEdge.m_a];
		list<Front::Vertex>::iterator B = otherFront->m_vertexIndexToFrontIterator[hitEdge.m_b];
						
		if (hitFront != currentFront)
		{
			// WHAT's THIS TEST ABOUT AGAIN?
			list<Front::Vertex>::const_iterator vertex = front->m_vertices.begin();
			while (vertex != front->m_vertices.end())
			{	
				if (A->m_index == vertex->m_index) 
				{
					return false;
				}
				if (B->m_index == vertex->m_index) 
				{
					return false;
				}
				vertex++;
			}
		}
		
		printf("-----------------------------\nBEGIN SPLITMERGE\n");
		debug_printFronts();
		
		{
			int debug_frontVertex = frontVertex->m_index;
			
				// debug
			list<Front::Vertex>::iterator debug_this = front->m_vertexIndexToFrontIterator[frontVertex->m_index];
			list<Front::Vertex>::iterator debug_prev = front->m_vertexIndexToFrontIterator[prevVertex->m_index];
			list<Front::Vertex>::iterator debug_next = front->m_vertexIndexToFrontIterator[nextVertex->m_index];
			
			int debug_this_index = debug_this->m_index;
			int debug_prev_index = debug_prev->m_index;
			int debug_next_index = debug_next->m_index;
			
			debug_next_index++;
		}
		
		// DEBUGGING
		{
			int debug_frontVertex = frontVertex->m_index;
			printf("%d\n", debug_frontVertex);
			 
			int debug_prevVertex = prevVertex->m_index;
			printf("%d\n", debug_prevVertex);
			
			int debug_nextVertex = nextVertex->m_index;
			printf("%d\n", debug_nextVertex);
			
			int debug_A = A->m_index;
			printf("%d\n", debug_A);
			
			int debug_B = B->m_index;
			printf("%d\n", debug_B);
			
			if (debug_A != hitEdge.m_a)
			{
				assert(0);
			}
			if (debug_B != hitEdge.m_b)
			{
				assert(0);
			}
		}
		
		const V3f& Va = m_mesh.m_vertices[A->m_index];
		const V3f& Vb = m_mesh.m_vertices[B->m_index];
		
		// Choose which vertices to connect; one of (prev, this, next) on the growing front, and one of the hit edge vertices.
		// We eliminate connections which collide, and then choose the shortest one.
		// But if all collide, the best we can do is just choose the shortest one. 
		FatEdge connections[6];
		
		int numCandidates = 0;
		
		assert((frontVertex->m_index!=A->m_index) && (frontVertex->m_index!=B->m_index));
		
		if ( (frontVertex->m_index!=A->m_index) && (prevVertex->m_index!=A->m_index) && (nextVertex->m_index!=A->m_index) ) 
			connections[numCandidates++] = FatEdge(frontVertex->m_index, A->m_index, m_mesh, m_vertexRocs);
		
		if ( (frontVertex->m_index!=B->m_index) && (prevVertex->m_index!=B->m_index) && (nextVertex->m_index!=B->m_index) ) 
			connections[numCandidates++] = FatEdge(frontVertex->m_index, B->m_index, m_mesh, m_vertexRocs);
		
			//if ( (prevVertex->m_index!=A->m_index) && (frontVertex->m_index!=A->m_index) )	connections[numCandidates++] = FatEdge(prevVertex->m_index,  A->m_index, m_mesh, m_vertexRocs);
			//if ( (prevVertex->m_index!=B->m_index) && (frontVertex->m_index!=B->m_index) )	connections[numCandidates++] = FatEdge(prevVertex->m_index,  B->m_index, m_mesh, m_vertexRocs);
			//if ( (nextVertex->m_index!=A->m_index) && (frontVertex->m_index!=A->m_index) )    connections[numCandidates++] = FatEdge(nextVertex->m_index,  A->m_index, m_mesh, m_vertexRocs);
			//if ( (nextVertex->m_index!=B->m_index) && (frontVertex->m_index!=B->m_index) )    connections[numCandidates++] = FatEdge(nextVertex->m_index,  B->m_index, m_mesh, m_vertexRocs);
		
		vector<int> potentialConnections;
		for (int c=0; c<numCandidates; ++c)
		{
			int anotherHitFront;
			FatEdge anotherHitEdge;
			if ( candidateEdgeFits(connections[c], anotherHitFront, anotherHitEdge, 1.0f) )
			{
				potentialConnections.push_back(c);
			}
		}
		
		if (potentialConnections.size()==0)
		{
			// All collide..  so we just put all of them in the bag for length testing.
			for (int c=0; c<numCandidates; ++c)
			{
				potentialConnections.push_back(c);
			}
		}
		
		if (numCandidates==0)
		{
			return false;
		}
		
		float minDist = FLT_MAX;
		int vA = -1; // vertex on the current front
		int vB = -1; // vertex on the other front
		for (int i=0; i<potentialConnections.size(); ++i)
		{
			const FatEdge& connection = connections[potentialConnections[i]];
			if (connection.m_a==connection.m_b) continue;
			const V3f& Ca = m_mesh.m_vertices[connection.m_a];
			const V3f& Cb = m_mesh.m_vertices[connection.m_b];
			
			// NO!! SHOULD CHOOSE ONE WITH LARGEST EDGE-EDGE PROXIMITY
			// OR ONE WHICH MAKES THE BEST QUALITY TRIANGLE
			float dist = (Ca - Cb).length();   
			if (dist<minDist)
			{
				minDist = dist;
				vA = connection.m_a;
				vB = connection.m_b;
			}
		}

		assert(vA!=-1);
		assert(vB!=-1);
		
		// Now we know which vertices to connect. We now have to update the front topology by either splitting or merging fronts.
		
		////////////////////////////////////////////////////////////////////////////////////////////////////////////
		// Front split
		////////////////////////////////////////////////////////////////////////////////////////////////////////////
		if (hitFront == currentFront)
		{
			printf("-------------- hitFront == currentFront = %d ---------------", currentFront);
			printf("\nSPLIT\n");
			
			// Split existing front F into two. Say the old vertex order in F was: 
			// F: a -> a_aft -> ... -> b_bef -> b -> b_aft -> ... a_bef -> wraparound to a
			
			// We create two new fronts F2, F2 to replace F:
			// F0: ...  a -> b -> b_aft -> ... -> a_bef -> wraparound to a
			// F1: ...  b -> a -> a_aft -> ... -> b_bef -> wraparound to b
			
			// Build F0
			Front* F0 = new Front();
			{
				// Start at vB
				list<Front::Vertex>::iterator iterF = front->m_vertexIndexToFrontIterator[vB];
				
				// Add to F0 all vertices in front starting at vB until (and including) vA
				while (1)
				{
					int debug_0 = iterF->m_index;
					
					F0->m_vertices.push_back(Front::Vertex(iterF->m_index));
					list<Front::Vertex>::iterator iterF0 = F0->m_vertices.end(); iterF0--;
					F0->m_vertexIndexToFrontIterator[iterF->m_index] = iterF0;
					
					int debug_1 = iterF0->m_index;
					
					if (iterF->m_index == vA) break;
					if (++iterF == front->m_vertices.end()) iterF++;
				}
			}
			
			// Build F1
			Front *F1 = new Front();
			{
				// Start at vA
				list<Front::Vertex>::iterator iterF = front->m_vertexIndexToFrontIterator[vA];
				
				// Add to F1 all vertices in front starting at vA until (and including) vB
				while (1)
				{
					int debug_0 = iterF->m_index;
					
					F1->m_vertices.push_back(Front::Vertex(iterF->m_index));
					list<Front::Vertex>::iterator iterF1 = F1->m_vertices.end(); iterF1--;
					F1->m_vertexIndexToFrontIterator[iterF->m_index] = iterF1;
					
					int debug_1 = iterF1->m_index;
					
					if (iterF->m_index == vB) break;
					if (++iterF == front->m_vertices.end()) iterF++;
				}
			}
			
			// TO DO: if one or both of the fronts has <= 4 vertices, we can dispense with it now.
			delete m_fronts[currentFront];
			m_fronts.erase(m_fronts.begin() + currentFront);
			
			bool bridgeNeeded = (F0->m_vertices.size()>4) && (F1->m_vertices.size()>4); 
			
			if (F0->m_vertices.size()>4)
			{
				if (0)//bridgeNeeded)
				{
					// Cut ear to construct (half of) front-front bridge: make triangle (a, b, b+)
					list<Front::Vertex>::iterator frontVertex = F0->m_vertexIndexToFrontIterator[vB];
					
					list<Front::Vertex>::iterator prevVertex = frontVertex; prevVertex--;
					if (prevVertex == F0->m_vertices.end()) prevVertex--;
					
					list<Front::Vertex>::iterator nextVertex = frontVertex; nextVertex++;
					if (nextVertex == F0->m_vertices.end()) nextVertex++;
					
						// Modify the front vertex list
					F0->m_vertexIndexToFrontIterator.erase(frontVertex->m_index);
					F0->m_vertices.erase(frontVertex);
					
						// Add new triangle to mesh
					{
						Geometry::Triangle tri;
						tri.m_vertex[0] = frontVertex->m_index;
						tri.m_vertex[1] = prevVertex->m_index;
						tri.m_vertex[2] = nextVertex->m_index;
						m_mesh.m_triangles.push_back(tri);
					}					
				}
				
				// Rebuild the front BVH
				F0->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);

				m_fronts.push_back(F0);
			}
			else
			{
				fillHole(F0);
				delete F0;
			}
			
			if (F1->m_vertices.size()>4)
			{
				if (0)//bridgeNeeded)
				{
						// Cut ear to construct (half of) front-front bridge: make triangle (a, a+, b)
					list<Front::Vertex>::iterator frontVertex = F1->m_vertexIndexToFrontIterator[vA];
					
					list<Front::Vertex>::iterator prevVertex = frontVertex; prevVertex--;
					if (prevVertex == F1->m_vertices.end()) prevVertex--;
					
					list<Front::Vertex>::iterator nextVertex = frontVertex; nextVertex++;
					if (nextVertex == F1->m_vertices.end()) nextVertex++;
					
						// Modify the front vertex list
					F1->m_vertexIndexToFrontIterator.erase(frontVertex->m_index);
					F1->m_vertices.erase(frontVertex);
					
						// Add new triangle to mesh
					{
						Geometry::Triangle tri;
						tri.m_vertex[0] = frontVertex->m_index;
						tri.m_vertex[1] = prevVertex->m_index;
						tri.m_vertex[2] = nextVertex->m_index;
						m_mesh.m_triangles.push_back(tri);
					}
				}
			
				// Rebuild the front BVH
				F1->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);
		
				m_fronts.push_back(F1);
			}
			else
			{
				fillHole(F1);
				delete F1;
			}
		}
		
		////////////////////////////////////////////////////////////////////////////////////////////////////////////
		// Front merge
		////////////////////////////////////////////////////////////////////////////////////////////////////////////
		else
		{
			printf("-------------- hitFront = %d != currentFront = %d ---------------", hitFront, currentFront);
			printf("\nMERGE\n");
			// Merge the two fronts FA and FB into one new front, F.
			
			// Say the old front vertex orders were:
			// FA: a -> a_aft -> ... -> a_bef -> wraparound to a
			// FB: b -> b_aft -> ... -> b_bef -> wraparound to a
			
			// We create one new front F to replace FA and FB:
			// F: a -> b_aft -> ... -> b_bef -> b -> a_aft -> ... -> a_bef -> wraparound to a
			
			// and a bridge of triangles (a, a+, b), (a, b, b+)
			
			// Build F
			Front* F = new Front();
			{
				// Start at the current vertex on this front (vA)
				F->m_vertices.push_back(Front::Vertex(vA));
				{
					list<Front::Vertex>::iterator iterF = F->m_vertices.end(); iterF--;
					int debug_0 = iterF->m_index;
					F->m_vertexIndexToFrontIterator[vA] = iterF;
				}
				
				// Add all vertices in the other front, starting at (vB+1)
				list<Front::Vertex>::iterator iterFB = otherFront->m_vertexIndexToFrontIterator[vB];
				if (++iterFB == front->m_vertices.end()) iterFB++;
				while (1)
				{
					F->m_vertices.push_back(Front::Vertex(iterFB->m_index));
					{
						list<Front::Vertex>::iterator iterF = F->m_vertices.end(); iterF--;
						int debug_0 = iterF->m_index;
						F->m_vertexIndexToFrontIterator[iterF->m_index] = iterF;
					}
					if (++iterFB == otherFront->m_vertices.end()) iterFB++;
					if (iterFB->m_index == vB) break;
				}
				
				// Add vB
				F->m_vertices.push_back(Front::Vertex(vB));
				{
					list<Front::Vertex>::iterator iterF = F->m_vertices.end(); iterF--;
					int debug_0 = iterF->m_index;
					F->m_vertexIndexToFrontIterator[vB] = iterF;
				}
				
				// Add all vertices in this front, starting at (vA+1)
				list<Front::Vertex>::iterator iterFA = front->m_vertexIndexToFrontIterator[vA];
				if (++iterFA == front->m_vertices.end()) iterFA++;
				while (1)
				{
					F->m_vertices.push_back(Front::Vertex(iterFA->m_index));
					{
						list<Front::Vertex>::iterator iterF = F->m_vertices.end(); iterF--;
						int debug_0 = iterF->m_index;
						F->m_vertexIndexToFrontIterator[iterF->m_index] = iterF;
					}
					if (++iterFA == front->m_vertices.end()) iterFA++;
					if (iterFA->m_index == vA) break;
				}
			}
			
			delete m_fronts[currentFront];
			delete m_fronts[hitFront];

			vector<Front*> newFronts;
			for (int f=0; f<m_fronts.size(); ++f)
			{
				if (f!=currentFront && f!=hitFront)
				{
					newFronts.push_back(m_fronts[f]);
				}
			}
			m_fronts = newFronts;
			
			if (F->m_vertices.size()>4)
			{
				m_fronts.push_back(F);

				// construct front-front bridge
				list<Front::Vertex>::iterator iterA = F->m_vertexIndexToFrontIterator[vA];
				list<Front::Vertex>::iterator iterAp = iterA; if (++iterAp == F->m_vertices.end()) iterAp++;
				list<Front::Vertex>::iterator iterB = F->m_vertexIndexToFrontIterator[vB];
				list<Front::Vertex>::iterator iterBp = iterB; if (++iterBp == F->m_vertices.end()) iterBp++;

				{   // (a, a+, b)
					Geometry::Triangle tri;
					tri.m_vertex[0] = iterA->m_index;
					tri.m_vertex[1] = iterAp->m_index;
					tri.m_vertex[2] = iterB->m_index;
					m_mesh.m_triangles.push_back(tri);
				}
				{   // (a, b, b+)
					Geometry::Triangle tri;
					tri.m_vertex[0] = iterA->m_index;
					tri.m_vertex[1] = iterB->m_index;
					tri.m_vertex[2] = iterBp->m_index;
					m_mesh.m_triangles.push_back(tri);
				}

				F->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);
			}
			else
			{
				assert(0);   // this should never happen, right?
				fillHole(F);
				delete F;
			}
		}
		
		printf("END SPLITMERGE\n");
		debug_printFronts();
		
		return true;
	}
	
	
	list<Front::Vertex>::iterator chooseFrontVertex(Front* front)
	{
			// AH.... NO...    OPENING ANGLES OF VERTICES WILL
			// CHANGE IF THEIR NEIGHBOUR TOPOLOGY CHANGES..
			// THIS IS NOT CURRENTLY TAKEN INTO ACCOUNT.
		
		
		float minOpeningAngle = FLT_MAX;
		list<Front::Vertex>::iterator vertex = front->m_vertices.begin();
		list<Front::Vertex>::iterator minAngleVertex = vertex;
		while (vertex != front->m_vertices.end())
		{	
			float openingAngle = vertex->m_openingAngle;
			if (openingAngle < minOpeningAngle)
			{
				minOpeningAngle = openingAngle;
				minAngleVertex = vertex;
			}
			vertex++;
		}
		return minAngleVertex;
	}
	
	
	template<typename Functor>
	bool grow()
	{			
		/*
		if (m_switchFront)
		{
			int numFronts = m_fronts.size();
			m_currentFront++;
			if (m_currentFront==numFronts) m_currentFront=0;
			m_currentFrontVertex = m_fronts[m_currentFront].m_vertices.begin();
			m_switchFront = false;
		}
		 */
		
		if (m_fronts.size()==0) return false;
	
		Front* front = m_fronts[m_currentFront];
		if (front->m_vertices.size()<=4)
		{
			fillHole(front);
			delete m_fronts[m_currentFront];
			m_fronts.erase(m_fronts.begin() + m_currentFront);
			
			m_switchFront = true;
			
			if (m_switchFront)
			{
				int numFronts = m_fronts.size();
				m_currentFront++;
				if (m_currentFront>=numFronts) m_currentFront=0;
				m_currentFrontVertex = m_fronts[m_currentFront]->m_vertices.begin();
				m_switchFront = false;
			}
			
			return false;
		}
		
		list<Front::Vertex>::iterator frontVertex = m_currentFrontVertex;
		if (frontVertex == front->m_vertices.end())
		{
			m_switchFront = true;
			
			if (m_switchFront)
			{
				int numFronts = m_fronts.size();
				m_currentFront++;
				if (m_currentFront>=numFronts) m_currentFront=0;
				m_currentFrontVertex = m_fronts[m_currentFront]->m_vertices.begin();
				m_switchFront = false;
			}
			
			return false;
		}
		
		printf("----------------  grow  -----------------\n\n");
		
			//debug_printFronts();
		
		// Choose best front vertex
		//list<Front::Vertex>::iterator minAngleVertex = chooseFrontVertex(front);
			//frontVertex = minAngleVertex;
		
		list<Front::Vertex>::iterator prevVertex = frontVertex; prevVertex--;
		if (prevVertex == front->m_vertices.end()) prevVertex--;
		
		list<Front::Vertex>::iterator nextVertex = frontVertex; nextVertex++;
		if (nextVertex == front->m_vertices.end()) nextVertex++;
		
		int debug_frontVertex = frontVertex->m_index;
		printf("%d\n", debug_frontVertex);
		
		int debug_prevVertex = prevVertex->m_index;
		printf("%d\n", debug_prevVertex);
		
		int debug_nextVertex = nextVertex->m_index;
		printf("%d\n", debug_nextVertex);
		
		
		
		int numVertices = m_mesh.m_vertices.size();
		int numNormals = m_mesh.m_vertexNormals.size();
		
		
		const V3f& V = m_mesh.m_vertices[frontVertex->m_index];
		const V3f& N = m_mesh.m_vertexNormals[frontVertex->m_index];
		
		const V3f& Vprev = m_mesh.m_vertices[prevVertex->m_index];
		const V3f& Vnext = m_mesh.m_vertices[nextVertex->m_index];
		
		V3f X, Y;
		computeTangentSpace(N, X, Y);
		
		V3f Vprev_project(0);
		Vprev_project.x = (V - Vprev) ^ X;
		Vprev_project.y = (V - Vprev) ^ Y;
		Vprev_project.normalize();
		
		V3f Vnext_project(0);
		Vnext_project.x = (Vnext - V) ^ X;
		Vnext_project.y = (Vnext - V) ^ Y;
		Vnext_project.normalize();
		
		float dot = clamp(Vprev_project ^ Vnext_project, -1.f+FLT_EPSILON, 1.f-FLT_EPSILON);
		
		float openingAngle;
		if ((Vprev_project % Vnext_project).z >= 0.0f)
		{
				// reflex angle
			openingAngle = M_PI + acos(dot);
		}
		else
		{
				// acute angle
			openingAngle = M_PI - acos(dot); 
		}
		
		frontVertex->m_openingAngle = openingAngle;
		
		int numPoints = floor(3.0*openingAngle/M_PI);
		
		const float EDGE_TOL = 0.1f;
		
		////////////////////////////////////////////////////////////////////////////////////
		// Attempt to cut ear first
		////////////////////////////////////////////////////////////////////////////////////
		//if (numPoints==0)
		
		float roc = m_vertexRocs[frontVertex->m_index];
	
		if ((openingAngle<0.666f*M_PI &&  ((Vprev - Vnext).length2() < 4.f*roc*roc)) || (numPoints==0))
		{
			// Try to "bridge the gap" ..
			
			// Check if the bridge is admissable.. check its fat edge fits
			FatEdge bridge(prevVertex->m_index, nextVertex->m_index, m_mesh, m_vertexRocs);
			
			FatEdge hitEdge;
			int hitFront;
			
			if ( candidateEdgeFits(bridge, hitFront, hitEdge, EDGE_TOL) || (numPoints==0) )
			{
				// Modify the front vertex list
				front->m_vertexIndexToFrontIterator.erase(frontVertex->m_index);
				front->m_vertices.erase(frontVertex);
			
				// Add new triangle to mesh
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = prevVertex->m_index;
					tri.m_vertex[2] = nextVertex->m_index;
					m_mesh.m_triangles.push_back(tri);
				}
				
				// Rebuild the front BVH
				front->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);
				
				prevVertex->m_active = true;
				nextVertex->m_active = true;
				
				m_currentFrontVertex = nextVertex;
				return true;
			}
			
#if 0
			else
			{
				frontVertex->m_active = false;
				
				/*
				bool frontStalled = true;
				list<Front::Vertex>::const_iterator vertex = front->m_vertices.begin();
				while (vertex != front->m_vertices.end())
				{	
					if (vertex->m_active)
					{
						frontStalled = false;
						break;
					}
					vertex++;
				}
				 */
				
					//if (frontStalled)
				{
					// We have a front-front collision to deal with.
					if ( splitOrMerge(m_currentFront, frontVertex, hitFront, hitEdge) )
					{
						m_switchFront = true;
						
						if (m_switchFront)
						{
							int numFronts = m_fronts.size();
							m_currentFront++;
							if (m_currentFront>=numFronts) m_currentFront=0;
							m_currentFrontVertex = m_fronts[m_currentFront]->m_vertices.begin();
							m_switchFront = false;
						}
					}
					else
					{
						m_currentFrontVertex = nextVertex;
					}
				}
					//else
					//{
					//	m_currentFrontVertex = nextVertex;
					//}
				
				
			}
#endif			
	
		}
		
		
		////////////////////////////////////////////////////////////////////////////////////
		// Attempt to add triangle fan
		///////////////////////////////////////////////////////////////////////////////////
		{
			// Extend the mesh with a fan of numPoints new vertices
			vector<V3f> newPoints;
			float roc = m_vertexRocs[frontVertex->m_index];
			float angle = openingAngle / float(numPoints+1);
			for (int n=0; n<numPoints; ++n)
			{
				V3f newPoint_project = rotateZ(-Vprev_project, float(n+1)*angle);
				V3f newPoint_dir = (newPoint_project.x*X + newPoint_project.y*Y).normalize();
				V3f newPoint = V + (2.f*roc*newPoint_dir);
				V3f projectedPoint = project<Functor>(newPoint, m_tol);
				newPoints.push_back(projectedPoint);
			}
			
			// Do an overlap test with the fattened fan edges, to check whether the fan is admissable.
			vector<float> newVertexRocs;
			for (int n=0; n<numPoints; ++n)
			{
				newVertexRocs.push_back( rocAtPoint<Functor>(newPoints[n]) );
			}
			vector<FatEdge> fanEdges;
			for (int n=0; n<numPoints; ++n)
			{
				fanEdges.push_back( FatEdge(frontVertex->m_index, newPoints[n], newVertexRocs[n], m_mesh, m_vertexRocs) );
			}
			for (int n=0; n<numPoints-1; ++n)
			{
				fanEdges.push_back( FatEdge(newPoints[n], newPoints[n+1], newVertexRocs[n], newVertexRocs[n+1]) );
			}
			fanEdges.push_back( FatEdge(prevVertex->m_index, newPoints[0], newVertexRocs[0], m_mesh, m_vertexRocs) );
			fanEdges.push_back( FatEdge(nextVertex->m_index, newPoints[numPoints-1], newVertexRocs[numPoints-1], m_mesh, m_vertexRocs) );
			
			bool fanFits = true;
			FatEdge hitEdge;
			int hitFront;
			for (int n=0; n<fanEdges.size(); ++n)
			{
				const FatEdge& fanEdge = fanEdges[n];
				if ( !candidateEdgeFits(fanEdge, hitFront, hitEdge, EDGE_TOL) )
				{
					fanFits = false;
					break;
				}
			}
			
			if (fanFits)
			{
				// If so, add the new vertices and triangles to the mesh, and modify the front accordingly.
				int vertexNum = m_mesh.m_vertices.size();
				
				// Add new vertices to mesh
				for (int n=0; n<numPoints; ++n)
				{
					m_mesh.m_vertices.push_back(newPoints[n]);
					m_vertexRocs.push_back(newVertexRocs[n]);
				}
				
				// Add new triangles to mesh
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = prevVertex->m_index;
					tri.m_vertex[2] = vertexNum+0;
					m_mesh.m_triangles.push_back(tri);
				}
				for (int n=0; n<numPoints-1; ++n)
				{
					Geometry::Triangle tri;                                                                                                                       
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = vertexNum+n;
					tri.m_vertex[2] = vertexNum+n+1;
					m_mesh.m_triangles.push_back(tri);
				}
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = vertexNum+numPoints-1;
					tri.m_vertex[2] = nextVertex->m_index;
					m_mesh.m_triangles.push_back(tri);
				}
				
				// Compute the new vertex normals
				for (int n=0; n<numPoints; ++n)
				{
					V3f N = gradient<Functor>(newPoints[n]);
					N.normalize();
					int debug_normal_index = m_mesh.m_vertexNormals.size();
					m_mesh.m_vertexNormals.push_back(N);
				}
				
				// Modify the front vertex list to insert the fan
				list<Front::Vertex>::iterator iter = nextVertex; 
				for (int n=numPoints-1; n>=0; n--)
				{
					Front::Vertex newVertex(vertexNum+n);
					iter = front->m_vertices.insert(iter, newVertex);
					front->m_vertexIndexToFrontIterator[vertexNum+n] = iter;
					computeOpeningAngle(iter, front);
				}
				front->m_vertexIndexToFrontIterator.erase(frontVertex->m_index);
				front->m_vertices.erase(frontVertex);
				
				prevVertex->m_active = true;
				nextVertex->m_active = true;
				
				// Rebuild the front BVH
				front->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);
				
				m_currentFrontVertex = nextVertex;
			}

			else if (openingAngle < 0.333333f*M_PI) // TO DO.. what is the basis for this angle then?
			{
				// Try to "bridge the gap" ..
				
				// TO DO: Another sanity check..  is the bridge length less than, say, twice the local edge length?
				
				// Check if the bridge is admissable.. check its fat edge fits
				FatEdge bridge(prevVertex->m_index, nextVertex->m_index, m_mesh, m_vertexRocs);
				int hitFront;
				FatEdge hitEdge;
				if ( candidateEdgeFits(bridge, hitFront, hitEdge, EDGE_TOL) )
				{
					// Modify the front vertex list
					front->m_vertexIndexToFrontIterator.erase(frontVertex->m_index);
					front->m_vertices.erase(frontVertex);
					
					// Add new triangle to mesh
					{
						Geometry::Triangle tri;
						tri.m_vertex[0] = frontVertex->m_index;
						tri.m_vertex[1] = prevVertex->m_index;
						tri.m_vertex[2] = nextVertex->m_index;
						m_mesh.m_triangles.push_back(tri);
					}
					
					// Rebuild the front BVH
					front->rebuildFatEdgeBVH(m_mesh, m_vertexRocs);
					
					prevVertex->m_active = true;
					nextVertex->m_active = true;
					
					m_currentFrontVertex = nextVertex;
				}
				else
				{
					frontVertex->m_active = false;
					m_currentFrontVertex = nextVertex;
					
					// We have a front-front collision to deal with.
					if ( splitOrMerge(m_currentFront, frontVertex, hitFront, hitEdge) )
					{
						m_switchFront = true;
						
						if (m_switchFront)
						{
							int numFronts = m_fronts.size();
							m_currentFront++;
							if (m_currentFront>=numFronts) m_currentFront=0;
							m_currentFrontVertex = m_fronts[m_currentFront]->m_vertices.begin();
							m_switchFront = false;
						}
					}
					else
					{
						m_currentFrontVertex = nextVertex;
					}
				}
			}
			 
			else
			{
				frontVertex->m_active = false;
				
				/*
				bool frontStalled = true;
				list<Front::Vertex>::const_iterator vertex = front->m_vertices.begin();
				while (vertex != front->m_vertices.end())
				{	
					if (vertex->m_active)
					{
						frontStalled = false;
						break;
					}
					vertex++;
				}
				 */
				
					//if (frontStalled)
				{
					// We have a front-front collision to deal with.
					if ( splitOrMerge(m_currentFront, frontVertex, hitFront, hitEdge) )
					{
						m_switchFront = true;
						
						if (m_switchFront)
						{
							int numFronts = m_fronts.size();
							m_currentFront++;
							if (m_currentFront>=numFronts) m_currentFront=0;
							m_currentFrontVertex = m_fronts[m_currentFront]->m_vertices.begin();
							m_switchFront = false;
						}
					}
					else
					{
						m_currentFrontVertex = nextVertex;
					}
				}
					//else
					//{
					//	m_currentFrontVertex = nextVertex;
					//}
			}

		}		
		
		return true;
	}
	
	template<typename Functor>
	void seedHexagon(V3f startPoint);
		
	
	V3d m_extents;
};


template<typename Functor>
void Polygonizer::seedHexagon(V3f startPoint)
{
	// Project it to the isosurface
	V3f seedPoint = project<Functor>(startPoint, m_tol);
	
	V3f grad = gradient<Functor>(seedPoint);
	
	V3f n = grad;
	n.normalize();
	
	V3f t, b;
	computeTangentSpace(n, t, b);
	
	float roc = rocAtPoint<Functor>(seedPoint);
	
	m_mesh.m_vertices.push_back(seedPoint);
	m_mesh.m_vertexNormals.push_back(n);
	m_vertexRocs.push_back(roc);
	
		// Generate initial front, which is directed anticlockwise from above
		// (i.e., the outside of the front is to the right as you walk along the directed front with your head in the normal/out direction).
	for (int i=0; i<6; ++i)
	{
		float x = 2.f * roc * cos(float(i) * 0.3333333f*M_PI);
		float y = 2.f * roc * sin(float(i) * 0.3333333f*M_PI);
		
		V3f projectedPoint = project<Functor>(seedPoint + x*t + y*b, m_tol);
		m_mesh.m_vertices.push_back(projectedPoint);
		
		V3f N = gradient<Functor>(projectedPoint);
		N.normalize();
		
		int debug_normal_index = m_mesh.m_vertexNormals.size();
		m_mesh.m_vertexNormals.push_back(N);
		
		m_vertexRocs.push_back( rocAtPoint<Functor>(projectedPoint) );
	}
	
	Front* front = new Front();
	for (int i=0; i<6; ++i)
	{
		int vertexIndex = 1 + (i%6);
		front->m_vertices.push_back(Front::Vertex(vertexIndex));
		
		list<Front::Vertex>::iterator iter = front->m_vertices.end();
		iter--;
		
		front->m_vertexIndexToFrontIterator[vertexIndex] = iter;
		computeOpeningAngle(iter, front);
	}
	m_fronts.push_back(front);
	
	for (int i=0; i<6; ++i)
	{
		Geometry::Triangle tri;                                                                                                                       
		tri.m_vertex[0] = 0;
		tri.m_vertex[1] = 1+(i%6);
		tri.m_vertex[2] = 1+((i+1)%6);
		m_mesh.m_triangles.push_back(tri);
	}	
}


	
template<typename Functor>
void Polygonizer::polygonize(const Box3f& bbox, float tol, float scale)
{
	
		// Look for a good start point
	V3f extents = bbox.max - bbox.min;
	m_extents = extents;
	const int startPointSearchIters = 64*64*64;
	float minValue = FLT_MAX;
	
	vector<V3f> seedPoints;
	
	V3f startPoint;
	for (int n=0; n<startPointSearchIters; ++n)
	{
		V3f testPoint = bbox.min + V3f(ran2(&m_seed)*extents[0], ran2(&m_seed)*extents[1], ran2(&m_seed)*extents[2]);
		float testVal = fabs(Functor::value(testPoint));
		
			//if (testVal<10.0)
			//{
			//	seedPoints.push_back(testPoint);
			//}
		
		if ( testVal<minValue ) 
		{
			startPoint = testPoint;
			minValue = testVal;
		}
	}
	
	seedPoints.push_back(startPoint);
	
	m_scale = scale;
	
	// Stamp initial hexagon
	m_tol = tol;
	
	for (int s=0; s<seedPoints.size(); ++s)
	{
		seedHexagon<Functor>(seedPoints[s]);
	}
		
	
	// DEBUG
	m_currentFront = 0;
	m_currentFrontVertex = m_fronts[0]->m_vertices.begin();
	m_switchFront = false;
}



#endif //POLYGONIZER3_H




#endif


